// 使用remez算法, 计算多项式f(x)=1.0/sqrt(x) 的最佳逼进3次多项式的系数
#include "remez.h"
#include "remez_invsqrt.h"
#include "linearEquationSolver.hpp"

using namespace mpfr;

#define DEFAULT_PRECISION 112
#define MPREAL_FORMAT "%.34RNf"

static void init_invsqrt_nodes(std::vector<mpreal> &nodes, mpreal a, mpreal b, int n )
{
    nodes.resize(n);

    mpreal step= (b-a)/mpreal(n);
    
    // 将n个节点均匀分布在区间[a,b]上
    for (int i = 0; i < n; i++)
    {
        nodes[i]=(a + step * mpreal(i));
    }
}

// 初始化线性方程组的系数
static void init_LinearEquationSolver_coeffs_v3(
    LinearEquationSolver &les, const std::vector<mpreal> nodes)
{
    int N = nodes.size();
    assert(les.getDims()==N);

    for (int i = 0; i < N; i++)
    {
        mpreal x = nodes[i];
        for (int j = 0; j < N-1; j++)
        {
            les.setCoefficient(i, j, pow(x, j) );
        }
        les.setCoefficient(i, N-1, ((i % 2 == 0) ? 1.0 : -1.0 ));
        les.set_rhs(i, invsqrt_func(x));
    }
}

// 用 Remez算法　计算invsqrt(x) 在区间 [a, b] 上的degree阶近似多项式,并返回多项式的系数
// 若invsqrt(x)的近似多项式　invsqrt(x)= c0 + c1*x + c2*x^2 + c3*x^3＋　(+/-)error
// 若degree=3,　需要计算c0,c1,c2,c3和error这5个未知数
// 由于只需要确定5个待定系数，所有需要构造1个5元一次方程组
static std::vector<mpreal> invsqrt_remez_algorithm_v3(
    mpreal &error,          // 传出参数，误差
    mpreal a, mpreal b,     // 区间[a,b]
    int degree, bool debug)
{
    std::vector<mpreal> nodes;
    std::vector<mpreal> coeffs;    // 多项式的系数=[c0,c1,c2,c3,error], 共degree+2个系数
    std::vector<mpreal> xs;        // 线性方程组的解

    int N = degree +2;             // 若degree=1,需要计算c0,c1,c2,c3和error这4个未知数

    LinearEquationSolver solver(N); // N个未知数，N个方程

    for (int round=0;1;round++ )
    {
        if (round==0)
            init_invsqrt_nodes(nodes,a, b, N); //使用均分节点

        if (debug)
        {
            std::cout <<"\nThe " << get_ordinal_number(round+1) << " iteration:";
            std::cout << "使用下列点，求最佳逼近多项式的系数:" << std::endl;
            print_nodes( nodes ,MPREAL_FORMAT);
        }

        init_LinearEquationSolver_coeffs_v3(solver, nodes);
        xs = solver.solve();

        coeffs.clear();
        for (int i = 0; i < N-1; i++)
            coeffs.push_back(xs[i]);
        error=xs[N-1];
        if ( debug)
        {
            std::cout << "得到的最佳逼近多项式的系数如下" << std::endl;
            print_coeffs( coeffs,MPREAL_FORMAT);
        }

        nodes= select_nodes(coeffs, invsqrt_error_derivative,invsqrt_get_error_at_x, a, b, degree,N,MPREAL_FORMAT); // 重新选择　N 个　节点

        mpreal min= const_infinity(1);  // positive infinity
        mpreal max= const_infinity(-1); // negative infinity
        for (int i = 0; i < N; i++)
        {
            std::string s_x =  nodes[i].toString(MPREAL_FORMAT);
            mpreal error    =  invsqrt_get_error_at_x( coeffs, nodes[i], degree);

            if ( abs(error)>max )
                max=abs(error);
            if ( abs(error)<min )
                min=abs(error);

            if (debug)
            {
                std::string s_error=  error.toString(MPREAL_FORMAT);
                std::cout << "At " << s_x << ",the error is " << s_error << std::endl;
            }
        }

        mpreal error_range_threshold = pow(mpreal("2"), -DEFAULT_PRECISION) * mpreal("64");
        if (debug)
        {
            std::string s0 = error_range_threshold.toString(MPREAL_FORMAT);  std::cout << "threshold:" << s0 << std::endl;
            std::string s1 = min.toString(MPREAL_FORMAT);  std::cout << "min:" << s1 << std::endl;
            std::string s2 = max.toString(MPREAL_FORMAT);  std::cout << "max:" << s2 << std::endl;
        }

        if (max-min<error_range_threshold)
            break;
    }
    return coeffs;
}

/*
  函数　my_invsqrt_coeffs_v3：
  使用Remez算法，计算 f(x)=1.0/sqrt(x)在区间 [a, b] 上的的最佳逼近多项式
  这里，区间 [a,b] = [1.0, 4.0]
  my_invsqrt(x)= c0 + c1*x + c2*x^2 + c3*x^3
  这里，多项式的阶数是2，我们通过Remez算法来求解c0,c1,c2,c3 这4个待定系数，需解5元一次线性方程组
*/
void my_invsqrt_coeffs_v3(bool debug)
{
    mpreal::set_default_prec(DEFAULT_PRECISION);

    int degree=3; // 多项式的阶数

    mpreal a = mpreal("1.0");  // a = 1;
    mpreal b = mpreal("4.0");  // b = 4.0;

    std::cout << "使用" << degree << "阶多式式，计算1/sqrt(x)的最佳切比雪夫最佳逼近" << std::endl;

    mpreal max_error;
    std::vector<mpreal> coeffs= invsqrt_remez_algorithm_v3(max_error,a, b,degree,debug);

    std::cout << "invsqrt(x)的Remez近似多项式系数 (x ∈ [" << a << "," << b << "]\n";
    for (int i = 0; i < coeffs.size(); i++)
    {
        std::string s= coeffs[i].toString(MPREAL_FORMAT);
        std::cout << "C" << i << " = " << s << std::endl;
    }
    std::string s= max_error.toString(MPREAL_FORMAT);
    std::cout << "max(error)=" << s << std::endl;

    std::string s_x =  max_error.toString(MPREAL_FORMAT);
    std::cout << "max(error)=" << s << std::endl;
}

